#!/usr/bin/env python
import rospy
import tf
import numpy as np
import transforms3d as tfs
import cv2
import numpy as np
from std_msgs.msg import Float32MultiArray
from tf2_ros.buffer_interface import convert

K = [[602.7175003324863, 0, 351.305582038406],
     [0, 601.6330312976042, 240.0929104708551],
     [0, 0, 1]]
D = [0.06712174262966401, -0.2636999208734844, 0.006484443443073637, 0.01111161327049835, 0]

MK = np.array(K, dtype=np.float)
MD = np.array(D, dtype=np.float)

z = 2.1 #meter
x,y = 0,0

def covert(point=[[0.0, 0.0]], z=1):
    point = np.array(point, dtype=np.float)
    pts_uv = cv2.undistortPoints(point, MK, MD)
    pts_uv = pts_uv * z
    return pts_uv[0][0]

def callback(data):
    global x,y,z
    print("callback recv fish_pose:",data.data)
    cx ,cy =  data.data
    print("cx,xy",cx,cy)
    print("cx,xy",type(cx),type(cy))
    pose_3d = covert([cx,cy],z)
    x,y,z = pose_3d[0],pose_3d[1],z


if __name__ == '__main__':
    rospy.init_node('xy2pose')   
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()  
    sub = rospy.Subscriber("fish_pose",Float32MultiArray,callback=callback,queue_size=10)

    rate = rospy.Rate(2.0)
    while not rospy.is_shutdown():
        try:
            (trans2,rot2) = [x,y , z], [0, 0, 0, 1]
            br.sendTransform(trans2,rot2,rospy.Time.now(),"fish","my_camera")
            # print(trans2,rot2)
            (trans1,rot1) = listener.lookupTransform('/base_link', '/fish', rospy.Time(0))
            print("base_link->fish",trans1,rot1)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        rate.sleep()
